#include "vehicle_system.h"

void VehicleSystem::init() {
  motorController->init();
  sensorManager->init();
  Serial.begin(9600);
  bufferIndex = 0;
  memset(commandBuffer, 0, sizeof(commandBuffer));
}

void VehicleSystem::processSerialCommands() {
  while (Serial.available() > 0) {
    char c = Serial.read();
    if (c == '\n' || bufferIndex >= sizeof(commandBuffer) - 1) {
      commandBuffer[bufferIndex] = '\0';
      processCommand(commandBuffer);
      bufferIndex = 0;
      memset(commandBuffer, 0, sizeof(commandBuffer));
    } else if (c != '\r') {
      commandBuffer[bufferIndex++] = c;
    }
  }
}

void VehicleSystem::processCommand(char* cmd) {
  if (strlen(cmd) == 0) return;

  if (cmd[0] == 'C') {
    processConfig(cmd + 1);
    return;
  }

  char direction = cmd[0];
  int speed = 0;
  if (strlen(cmd) > 1) {
    speed = atoi(cmd + 1);
  }

  switch (direction) {
    case 'F':
      motorController->forward(speed);
      break;
    case 'B':
      motorController->backward(speed);
      break;
    case 'L':
      motorController->turnLeft(speed);
      break;
    case 'R':
      motorController->turnRight(speed);
      break;
    case 'S':
      motorController->stop();
      break;
    default:
      break;
  }
}

void VehicleSystem::processConfig(char* config) {
  StaticJsonDocument<200> doc;
  DeserializationError error = deserializeJson(doc, config);
  if (error) return;

  if (doc.containsKey("max_speed")) {
    int newSpeed = doc["max_speed"];
    configManager->setMaxSpeed(newSpeed);
    motorController->setSpeed(newSpeed);
  }

  if (doc.containsKey("safe_distance")) {
    int newDistance = doc["safe_distance"];
    configManager->setSafeDistance(newDistance);
  }

  if (doc.containsKey("drive_mode")) {
    String newMode = doc["drive_mode"];
    configManager->setDriveMode(newMode);
  }
}

void VehicleSystem::updateSensors() {
  if (sensorManager->shouldUpdate()) {
    float distance = sensorManager->getDistance();
    float temperature = sensorManager->getTemperature();
    sensorManager->updateLastUpdate();

    StaticJsonDocument<200> doc;
    doc["distance"] = distance;
    doc["temperature"] = temperature;
    doc["drive_mode"] = configManager->getDriveMode();
    doc["max_speed"] = configManager->getMaxSpeed();
    doc["safe_distance"] = configManager->getSafeDistance();

    serializeJson(doc, Serial);
    Serial.println();
  }
}

void VehicleSystem::checkSafety() {
  float distance = sensorManager->getDistance();
  if (distance > 0 && distance < configManager->getSafeDistance()) {
    motorController->stop();
  }
}

void VehicleSystem::loop() {
  processSerialCommands();
  updateSensors();
  checkSafety();
}